/*
This file is part of MMM.

MMM is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

MMM is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with MMM.  If not, see <http://www.gnu.org/licenses/>.
*
* @package    MMM
* @author     Nikolaus Vahrenkamp
* @copyright  2013 High Performance Humanoid Technologies (H2T), Karlsruhe, Germany
*
*/

#ifndef __MMM_LegacyMotion_H_
#define __MMM_LegacyMotion_H_

#include "../../MMMCore.h"
#include "../../MMMImportExport.h"
#include "MotionEntries.h"
#include "MotionFrame.h"
#include "AbstractMotion.h"
#include "../../Model/Model.h"
#include "../../Model/ModelProcessor.h"

#include <Eigen/Core>
#include <string> 
#include <vector>
#include <map>
#include <fstream>

namespace MMM
{
class LegacyMotion;

/**
	@brief A Motion defines a series of MotionFrame snapshots.
	The data covers position, velocity and acceleration of the models root position and orientation 
	and a set of joint values, velocities and accelerations.
*/
typedef boost::shared_ptr<LegacyMotion> LegacyMotionPtr;
typedef std::vector<LegacyMotionPtr> LegacyMotionList;

class MMM_IMPORT_EXPORT LegacyMotion : public AbstractMotion
{
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    LegacyMotion(const std::string& name);

    LegacyMotion(const LegacyMotion& m);

	/*!
		Adds a snapshot to the motion data.
		The dimensions of the joint data is checked against the internal ndof value.
		The data is added without checking the timestep order.
		\param md The data to add.
		\return True on success.
	*/
	bool addMotionFrame(MotionFramePtr md);
    bool removeMotionFrame(size_t frame);

	/*!
		The joint names can be specified here. This implicitly sets the number of DoFs.
	*/
	bool setJointOrder(const std::vector<std::string> &jointNames);

    void setName(const std::string& name);

    //! Returns the name
    std::string getName();

	/*!
		Specifies a comment string.
		Overwrites current comment.
		Adds standard MotionEntry with name "comments".
	*/
    void setComment(const std::string &comment);
    void addComment(const std::string &comment);

    //! Returns comment strings combined to one std::string.
	std::string getComment();

    //! Returns the joint names.
    std::vector<std::string> getJointNames();

	/*!
		Returns the corresponding frame.
		If frame is out of bounds, an empty MotionFrame is returned.
	*/
	MotionFramePtr getMotionFrame(size_t frame);


	//! Returns the vector with all MotionFrame objects.
    std::vector<MotionFramePtr> getMotionFrames();

    //! Sets the vector with all MotionFrame objects;
    void setMotionFrames(std::vector<MotionFramePtr> &mfs){motionFrames = mfs;}

	/*!
		Returns the number of entries.
	*/
	virtual unsigned int getNumFrames();

	void setModel(ModelPtr model);
	void setModel(ModelPtr processedModel, ModelPtr originalModel);

    /*!
        Return the model.
        @param processedModel Indicates weather the processed or the original model should be returned (If no modelProcessor is specified, the originalModel is always returned).
        @return The model or ModelPtr() if no model is not specified.
    */
    ModelPtr getModel(bool processedModel = true);

    const std::string& getMotionFilePath();
    void setMotionFilePath(const std::string&filepath);
    //! returns the motion file name and extension
    const std::string& getMotionFileName();
    //! stores the motion file name and extension
    void setMotionFileName(const std::string&filename);

    const std::string &getModelFilePath();
    void setModelFilePath(const std::string& filepath);


	void setModelProcessor(ModelProcessorPtr mp);


    /*!
        Return the ModelProcessor (if specified).
    */
    ModelProcessorPtr getModelProcessor();

	/*!
		Creates an XML string for this object.
	*/
	virtual std::string toXML();

	//! Prints the XML string to std::cout
	void print();

    //! Prints the XML string to file
    void print(const std::string &filename);

    //! generate data for representing motions
    void outputData(const std::string &filename);

	//! Validates if a given joint name is present
	bool hasJoint( const std::string &name );

    //! Calculate Joint Velocities from Joint Values
    void calculateVelocities(int method = 0);

    //! Calculate Joint Accelerations from Joint Velocities
    void calculateAccelerations(int method = 0);

    //! Enumeration to specify set of joint parameters
    enum jointEnum{eValues, eVelocities, eAccelerations};

    //! Smooth out joint values, veloctities or accelerations (specifited by type) by applying a rolling mean filter of size windowSize
    void smoothJointValues(jointEnum type, int windowSize);

    //! get a segment motion from the original motion
    LegacyMotionPtr getSegmentMotion(size_t frame1, size_t frame2);

    LegacyMotionPtr copy();

    //! get a segmented motion list according to segment points
    LegacyMotionList getSegmentMotions(std::vector<int> segmentPoints);

    //! join two motions
    void joinMotion(LegacyMotionPtr next);

    //! set the start position of the mmm model in the motion
    void setStartPosition(const Eigen::Vector3f &startPosition);

    //! set the start pose of the mmm model in the motion
    void setStartPose(const Eigen::Matrix4f &startPose);

    //! get the time stamps of a motion
    std::vector<double> getTimestamps();

    //! set start time point
    void setStartTime(double start, double step = 0.01);
protected:

    std::vector<MotionFramePtr> motionFrames;
	std::vector<std::string> jointNames;
	std::string name;

    ModelPtr model; // the processed model
    ModelPtr originalModel;
    ModelProcessorPtr modelProcessor;
    std::string motionFilePath;
    std::string motionFileName; //the motion file name and extension
    std::string modelFilePath;

private:
    Eigen::MatrixXf getJointValuesAsMatrix();
    Eigen::MatrixXf getJointVelocitiesAsMatrix();
    Eigen::MatrixXf getJointAccelerationsAsMatrix();
    Eigen::MatrixXf calculateDifferentialQuotient(const Eigen::MatrixXf &inputMatrix, int method = 0);
    Eigen::VectorXf applyRollingMeanSmoothing(Eigen::VectorXf &input, int windowSize);
    Eigen::MatrixXf calculateDerivativeOverSpline(const Eigen::MatrixXf &inputMatrix, bool firstOrder); //firstOrder true for first order derivative (velocity), false for second order (acceleration)
};



}

#endif
